#include <iostream>
#include "kalman.h"
using namespace std;

KalmanFiter::KalmanFiter(int num_beams)
{
    // 初始化卡尔曼滤波器参数
    state = Eigen::VectorXd(num_beams);
    state.setZero();
    covariance = Eigen::MatrixXd::Identity(num_beams, num_beams);
    F = Eigen::MatrixXd::Identity(num_beams, num_beams);
    H = Eigen::MatrixXd::Identity(num_beams, num_beams);
    R = Eigen::MatrixXd::Identity(num_beams, num_beams) * 0.1;
    Q = Eigen::MatrixXd::Identity(num_beams, num_beams) * 0.1;
}
void KalmanFiter::predict()
{
    // 预测步骤
    state = F * state;
    covariance = F * covariance * F.transpose() + Q;
}
void KalmanFiter::update(const Eigen::VectorXd &measurement)
{
    // 更新步骤
    Eigen::VectorXd y = measurement - H * state;
    Eigen::MatrixXd S = H * covariance * H.transpose() + R;
    Eigen::MatrixXd K = covariance * H.transpose() * S.inverse();

    state = state + K * y;
    covariance = (Eigen::MatrixXd::Identity(state.size(), state.size()) - K * H) * covariance;
}
Eigen::VectorXd KalmanFiter::getState() const
{
    return state;
}